#include "dispensehead.h"

DispenseImageProcessor::DispenseImageProcessor(DispenseHead *dispHead) : dispHead(dispHead) {}

void DispenseImageProcessor::setImageProcessType(DispenseImageProcessor::ImageProcessType type)
{
    imageProcessType = type;
}

void DispenseImageProcessor::processPr(QImage &image, SensorTrayMaterialData *materialData)
{
    {
        QMutexLocker tmpLocker(&locker);
        prProcessingDatas.enqueue(PrProcessingData(image, materialData));
    }
    start();
}

void DispenseImageProcessor::processGlueCheck(QImage &imageBefor,
                                              QImage &imageAfter,
                                              SensorTrayMaterialData *materialData)
{
    {
        QMutexLocker tmpLocker(&locker);
        glueCheckDatas.enqueue(GlueCheckData(imageBefor, imageAfter, materialData));
    }
    start();
}

void DispenseImageProcessor::run()
{
    if (imageProcessType == Pr)
    {
        while (true)
        {
            PrProcessingData prProcessingData;
            {
                QMutexLocker tmpLocker(&locker);
                if (prProcessingDatas.isEmpty())
                {
                    return;
                }
                prProcessingData = prProcessingDatas.dequeue();
            }

            PrOffset prOffset;
            if (dispHead->trayRawSensorLocation->performPR(prProcessingData.image, prOffset, true))
            {
                prProcessingData.materialData->setDispPrOffsetT(prOffset.Theta);
                prProcessingData.materialData->setDispPrOffsetOriginX(prOffset.O_X);
                prProcessingData.materialData->setDispPrOffsetOriginY(prOffset.O_Y);
                prProcessingData.materialData->setMaterialState(SensorTrayMaterialData::DispPrOk);
            }
            else
            {
                prProcessingData.materialData->setMaterialState(SensorTrayMaterialData::DispPrNg);
            }

            if (!dispHead->disableLeftDh)
            {
                dispHead->tryToDistributeShotGlueTaskToLeftDH();
            }
        }
    }
    else if (imageProcessType == GlueCheck)
    {
        while (true)
        {
            GlueCheckData glueCheckData;
            {
                QMutexLocker tmpLocker(&locker);
                if (glueCheckDatas.isEmpty())
                {
                    return;
                }
                glueCheckData = glueCheckDatas.dequeue();
            }

            if (dispHead->glueCheck(glueCheckData.imageBefore, glueCheckData.imageAfter))
            {
                glueCheckData.materialData->setMaterialState(SensorTrayMaterialData::DispCheckOk);
            }
            else
            {
                glueCheckData.materialData->setMaterialState(SensorTrayMaterialData::DispCheckNg);
            }
        }
    }
}

DispenseHead::DispenseHead(bool isLeftDispHead,
                           DispenseHeadCommonConfig *dhCommonConfig,
                           HTG1Laser *laser,
                           TrayConfig *sensorTrayConfig,
                           SensorTrayMap *sensorTrayMap,
                           BasicElementFactory *motionElementFactory)
    : Worker(isLeftDispHead ? WorkerName::LeftDispenseHead : WorkerName::RightDispenseHead),
      isLeftDispHead(isLeftDispHead),
      dhCommonConfig(dhCommonConfig),
      laser(laser),
      sensorTrayConfig(sensorTrayConfig),
      sensorTrayMap(sensorTrayMap)
{
    dhConfig = new DispenseHeadConfig(this);
    dhConfigFile = new ConfigFile("DispenseHeadConfig", dhConfig,
                                  QString("%1/%2Config.json").arg(getWorkerConfigDir()).arg(workerName()));
    dhConfigFile->populate();

    dispenser = motionElementFactory->createDispenser(logCate(), dhCommonConfig->dispenserConfig());
    auto xtDispenser = qobject_cast<XtDispenser *>(dispenser);
    if (xtDispenser != nullptr)
    {
        xtDispenser->setXtFlyDispensingConfig(dhCommonConfig->xtFlyDispensingConfig());
    }

    if (!isLeftDispHead)
    {
        dispImageProcessor = new DispenseImageProcessor(this);
    }
}

void DispenseHead::setAnotherDispHead(DispenseHead *value)
{
    anotherDispHead = value;
}

void DispenseHead::initEventHandle()
{
    if (isLeftDispHead)
    {
        dispZ = MotionElementContainer::getIns()->getItem<SingleAxis>(SAxisModuleName::LeftDispZ);
        dispXYZ = MotionElementContainer::getIns()->getItem<XYZModule>(XYZModuleName::LeftDispXYZ);
        dispZLockCyl = MotionElementContainer::getIns()->getItem<SCCylinder>(CylName::LeftDispZLock);
        shotGlueOut = MotionElementContainer::getIns()->getItem<SCDO>(DoName::LeftShotGlueOut);
    }
    else
    {
        dispZ = MotionElementContainer::getIns()->getItem<SingleAxis>(SAxisModuleName::RightDispZ);
        dispXYZ = MotionElementContainer::getIns()->getItem<XYZModule>(XYZModuleName::RightDispXYZ);
        dispZLockCyl = MotionElementContainer::getIns()->getItem<SCCylinder>(CylName::RightDispZLock);
        shotGlueOut = MotionElementContainer::getIns()->getItem<SCDO>(DoName::RightShotGlueOut);
    }
    trayRawSensorLocation = VisionManager::getIns().getVisionLocation(VLName::TrayRawSensorLocation);
    laserToCameraOffsetCaliLocation
        = VisionManager::getIns().getVisionLocation(VLName::LaserToCameraOffsetCaliLocation);
    glueDotLocation = VisionManager::getIns().getVisionLocation(VLName::GlueDotLocation);
    trayRawSensorCalibration = VisionManager::getIns().getCalibration(CaliName::TrayRawSensorCalibration);

    dispenser->init(dispXYZ->xAxis(), dispXYZ->yAxis(), dispXYZ->zAxis(), shotGlueOut);
    if (!isLeftDispHead)
    {
        try
        {
            readDispensePath();
        }
        catch (SilicoolException &se)
        {
            qCCritical(logCate()) << se.what();
        }
    }
    //    laser->init();
}

void DispenseHead::startEventHandle(const WorkerEvent &event)
{
    Q_UNUSED(event);
    if (mechDispPath.isEmpty())
    {
        qCCritical(logCate()) << tr("Dispense path is empty!");
        pushEvent(WorkerEvent(SMD::Abort));
    }
    if (!isLeftDispHead)
    {
        pushEvent(WorkerEvent("moveToReadyPos", QVariant(), 1001));
    }
}

void DispenseHead::stopEventHandle()
{
    stopEventHandled();
}

void DispenseHead::purgeEventHandle()
{
    purgeEventHandled();
}

void DispenseHead::homeWorker(bool moveToReadyPosAfterHome)
{
    Q_UNUSED(moveToReadyPosAfterHome)
}

void DispenseHead::measureTrayLevel()
{
    checkIsInit();

    anotherDispHead->dispXYZ->moveTo(XYZPosName::StandbyPos);

    auto *leftTopPos = dispXYZ->getPos<XYZModulePos>(XYZPosName::MeasureTrayLevelLeftTop);
    auto *rightBottomPos = dispXYZ->getPos<XYZModulePos>(XYZPosName::MeasureTrayLevelRightBottom);

    QList<double> heights;
    QList<QString> lables;

    dispXYZ->moveToPos(XYZPosName::MeasureTrayLevelLeftTop, XYZModulePos::ZS_XY_Z);

    QThread::msleep(100);
    heights.append(laser->readValidHeight());
    lables.append("LeftTop");

    double xStep = rightBottomPos->xPos() - leftTopPos->xPos();
    dispXYZ->xAxis()->relMove(xStep);
    QThread::msleep(100);
    heights.append(laser->readValidHeight());
    lables.append("RightTop");

    dispXYZ->moveTo(XYZPosName::MeasureTrayLevelRightBottom);
    QThread::msleep(100);
    heights.append(laser->readValidHeight());
    lables.append("RightBottom");

    dispXYZ->xAxis()->relMove(-xStep);
    QThread::msleep(100);
    heights.append(laser->readValidHeight());
    lables.append("LeftBottom");

    int maxIndex = 0;
    int minIndex = 0;
    QString rawHeight;
    for (int i = 0; i < 4; i++)
    {
        if (heights[i] > heights[maxIndex])
        {
            maxIndex = i;
        }
        if (heights[i] < heights[minIndex])
        {
            minIndex = i;
        }
        rawHeight += QString("%1_%2 ").arg(lables[i]).arg(heights[i]);
    }

    QString msg = QString("Measure tray level successful!\r\nRaw heights: %1\r\nMax: %2, Min: %3, Range: %4")
                      .arg(rawHeight)
                      .arg(QString("%1_%2").arg(lables[maxIndex]).arg(heights[maxIndex]))
                      .arg(QString("%1_%2").arg(lables[minIndex]).arg(heights[minIndex]))
                      .arg(heights[maxIndex] - heights[minIndex]);
    UIOperation::getIns()->showTip(msg);

    dispXYZ->moveTo(XYZPosName::StandbyPos);
}

void DispenseHead::performAllSensorPr()
{
    checkIsInit();
    if (isLeftDispHead)
    {
        throw SilicolAbort(tr("Only right dispense head is able to performAllSensorPr!"), EX_LOCATION);
    }

    disableLeftDh = dhCommonConfig->disableLeftDispHead();
    disableRightDh = dhCommonConfig->disableRightDispHead();
    executeGlueCheck = dhCommonConfig->executeGlueCheck();

    dispImageProcessor->setImageProcessType(DispenseImageProcessor::Pr);

    dispXYZ->moveZToPos(XYZPosName::PrLeftTop, false);
    bool waitZArrivedPos = false;
    for (int columnIndex = 0; columnIndex < sensorTrayConfig->columns(); columnIndex++)
    {
        bool isDownToUp = (columnIndex % 2 == 1);    // 扫描顺序，从左上开始，弓形扫描
        for (int rowIndex = 0; rowIndex < sensorTrayConfig->rows(); rowIndex++)
        {
            int realRowIndex = isDownToUp ? sensorTrayConfig->rows() - 1 - rowIndex : rowIndex;
            auto materialData = sensorTrayMap->getData<SensorTrayMaterialData>(realRowIndex, columnIndex);
            if (materialData->materialState() == SensorTrayMaterialData::Init)
            {
                QPointF targetPos = getPrXyPos(realRowIndex, columnIndex);
                dispXYZ->moveXY(targetPos.x(), targetPos.y());
                if (!waitZArrivedPos)
                {
                    dispXYZ->waitZArrivedPos(XYZPosName::PrLeftTop);
                    waitZArrivedPos = true;
                }

                QImage img = trayRawSensorLocation->getImage();
                materialData->setDispPrPosX(targetPos.x());
                materialData->setDispPrPosY(targetPos.y());
                dispImageProcessor->processPr(img, materialData);

                if (executeGlueCheck)
                {
                    imagesBeforeDispense[index2String(realRowIndex, columnIndex)] = img;
                }
            }
            else
            {
                if (!disableLeftDh)
                {
                    tryToDistributeShotGlueTaskToLeftDH();
                }
            }
        }
    }

    while (!dispImageProcessor->wait(3000))
    {
        qCWarning(logCate()) << tr("Waiting pr processing...");
    }
    if (disableRightDh)
    {
        rightDH()->pushEvent(WorkerEvent("moveToStandbyPos", QVariant(), 3000));
    }
}

void DispenseHead::shotGlue(int rowIndex, int columnIndex)
{
    auto sensorTrayMaterialData = sensorTrayMap->getData<SensorTrayMaterialData>(rowIndex, columnIndex);
    if (sensorTrayMaterialData->materialState() != SensorTrayMaterialData::DispPrOk)
    {
        qCCritical(logCate()) << "Logic Error! Execute shoting glue task but sensor state is:"
                              << sensorTrayMaterialData->materialState();
        return;
    }
    auto willBeHandledByWhichDH = sensorTrayMaterialData->willBeHandledByWhichDH();
    if ((isLeftDispHead && willBeHandledByWhichDH != SensorTrayMaterialData::Left)
        || (!isLeftDispHead && willBeHandledByWhichDH != SensorTrayMaterialData::Right))
    {
        qCCritical(logCate()) << QString("Logic Error! Execute shoting glue task. This is: %1, sensor will be handled "
                                         "by: %2.")
                                     .arg(workerName())
                                     .arg(willBeHandledByWhichDH);
        return;
    }

    waitIsAbleToExecuteTask(sensorTrayMaterialData);
    if (hasAbortEvent())
    {
        return;
    }

    if (needMeasureHeight(true))
    {
        QPointF measureHeightPos = mapMeasureHeightPoint(sensorTrayMaterialData);
        dispZ->moveTo(SAxisPosName::MeasureHeightPos, false);
        dispXYZ->moveXY(measureHeightPos.x(), measureHeightPos.y());
        dispZ->waitArrivedPos(SAxisPosName::MeasureHeightPos);
        double laserHeight = laser->readValidHeight();
        double currentZ = dispZ->axis()->getFeedbackPos(-1);
        shotGlueZHeight = currentZ - (laserHeight - dhConfig->laserHeightWhileNozeelTouchedTable());
        hasMeasureHeightForThisTray = true;
    }
    auto measureHeightFre
        = DispenseHeadCommonConfig::MeasureHeightFreEnumInfo().nameToEnum(dhCommonConfig->measureHeightFre());
    double shotGlueHeight
        = measureHeightFre == DispenseHeadCommonConfig::OnlyOnce ? dhConfig->shotGlueZHeight() : shotGlueZHeight;
    shotGlueHeight += dhCommonConfig->shotGlueZOffset();

    auto dispensePath = mapDispensePath(sensorTrayMaterialData, shotGlueHeight);

    try
    {
        executeDispensePath(dispensePath);
        sensorTrayMaterialData->setMaterialState(SensorTrayMaterialData::DispenseOk);
    }
    catch (SilicoolException &se)
    {
        sensorTrayMaterialData->setMaterialState(SensorTrayMaterialData::DispenseNg);
        throw se;
    }
}

void DispenseHead::calibrateNozzelToCameraTipOffset()
{
    moveToShotGlueDot();
    auto *glueDotPrPos = rightDH()->dispXYZ->getPos<XYZModulePos>(XYZPosName::GlueDotPrPos);
    if (isLeftDispHead)
    {
        dispXYZ->moveToPos(XYZPosName::StandbyPos, XYZModulePos::Z_XY);
        anotherDispHead->dispXYZ->moveToPos(XYZPosName::GlueDotPrPos, XYZModulePos::ZS_XY_Z);
    }
    else
    {
        dispXYZ->moveToPos(XYZPosName::GlueDotPrPos, XYZModulePos::ZS_XY_Z);
    }
    PrOffset prOffset;
    if (!glueDotLocation->performPR(prOffset))
    {
        throw SilicolAbort("Perform glueDotLocation pr failed!");
    }

    auto *shotGlueDotPos = dispXYZ->getPos<XYZModulePos>(XYZPosName::ShotGlueDotPos);
    double offsetX = shotGlueDotPos->xPos() - (glueDotPrPos->xPos() - prOffset.X);
    double offsetY = shotGlueDotPos->yPos() - (glueDotPrPos->yPos() - prOffset.Y);

    auto res = UIOperation::getIns()->yesNoConfirm(tr("calibrateNozzelToCameraTipOffset successful!\r\nOffset x: "
                                                      "%1, Offset y: %2\r\nWould you like to apply this offset?")
                                                       .arg(offsetX)
                                                       .arg(offsetY));
    if (res)
    {
        dhConfig->setNozeelToCameraOffsetX(offsetX);
        dhConfig->setNozeelToCameraOffsetY(offsetY);
    }
}

void DispenseHead::calculateNozzelToCameraOffset()
{
    checkIsInit();

    auto res = UIOperation::getIns()->yesNoConfirm(
        tr("Make sure you have done the following steps, otherwise the nozzel to camera offset may be incorrect!\r\n1. "
           "Click moveToShotGlueDot button.\r\n2. Move camera center to glue dot center."));

    if (res)
    {
        double currentXPos = rightDH()->dispXYZ->xAxis()->getFeedbackPos(-1);
        double currentYPos = rightDH()->dispXYZ->yAxis()->getFeedbackPos(-1);
        auto *shotGlueDotPos = dispXYZ->getPos<XYZModulePos>(XYZPosName::ShotGlueDotPos);
        dhConfig->setNozeelToCameraOffsetX(shotGlueDotPos->xPos() - currentXPos);
        dhConfig->setNozeelToCameraOffsetY(shotGlueDotPos->yPos() - currentYPos);
    }
}

void DispenseHead::calibrateLaserToCameraTipOffset()
{
    checkIsInit();

    if (isLeftDispHead)
    {
        dispXYZ->moveToPos(XYZPosName::StandbyPos, XYZModulePos::Z_XY);
        anotherDispHead->dispXYZ->moveToPos(XYZPosName::LaserToCameraOffsetPrPos, XYZModulePos::ZS_XY_Z);
    }
    else
    {
        anotherDispHead->dispXYZ->moveToPos(XYZPosName::StandbyPos, XYZModulePos::Z_XY);
        dispXYZ->moveToPos(XYZPosName::LaserToCameraOffsetPrPos, XYZModulePos::ZS_XY_Z);
    }
    PrOffset prOffset;
    if (!laserToCameraOffsetCaliLocation->performPR(prOffset))
    {
        throw SilicolAbort(tr("laserToCameraOffsetCaliLocation pr failed!"));
    }
    if (isLeftDispHead)
    {
        anotherDispHead->dispXYZ->moveToPos(XYZPosName::StandbyPos, XYZModulePos::Z_XY);
    }

    auto laserToCameraOffsetLaserPos = dispXYZ->getPos<XYZModulePos>(XYZPosName::LaserToCameraOffsetLaserPos);

    dispXYZ->moveToPos(XYZPosName::LaserToCameraOffsetLaserPos, XYZModulePos::ZS_XY_Z);
    QThread::msleep(100);
    double targetX1
        = laserToCameraOffsetLaserPos->xPos() + abs(dhCommonConfig->laserToCameraOffsetCaliParam()->moveRange());
    double x1 = getAxisPosWhileLaserHeightChanged(dispXYZ->xAxis(), targetX1, "X+");

    dispXYZ->moveXYToPos(XYZPosName::LaserToCameraOffsetLaserPos);
    QThread::msleep(100);
    double targetX2
        = laserToCameraOffsetLaserPos->xPos() - abs(dhCommonConfig->laserToCameraOffsetCaliParam()->moveRange());
    double x2 = getAxisPosWhileLaserHeightChanged(dispXYZ->xAxis(), targetX2, "X-");

    dispXYZ->moveXYToPos(XYZPosName::LaserToCameraOffsetLaserPos);
    QThread::msleep(100);
    double targetY1
        = laserToCameraOffsetLaserPos->yPos() + abs(dhCommonConfig->laserToCameraOffsetCaliParam()->moveRange());
    double y1 = getAxisPosWhileLaserHeightChanged(dispXYZ->yAxis(), targetY1, "Y+");

    dispXYZ->moveXYToPos(XYZPosName::LaserToCameraOffsetLaserPos);
    QThread::msleep(100);
    double targetY2
        = laserToCameraOffsetLaserPos->yPos() - abs(dhCommonConfig->laserToCameraOffsetCaliParam()->moveRange());
    double y2 = getAxisPosWhileLaserHeightChanged(dispXYZ->yAxis(), targetY2, "Y-");

    double centerX = (x1 + x2) / 2;
    double centerY = (y1 + y2) / 2;
    dispXYZ->moveXY(centerX, centerY);

    auto *laserToCameraOffsetPrPos = rightDH()->dispXYZ->getPos<XYZModulePos>(XYZPosName::LaserToCameraOffsetPrPos);
    double offsetX = centerX - (laserToCameraOffsetPrPos->xPos() - prOffset.X);
    double offsetY = centerY - (laserToCameraOffsetPrPos->yPos() - prOffset.Y);

    auto res = UIOperation::getIns()->yesNoConfirm(tr("calibrateLaserToCameraTipOffset successful!\r\nOffset x: %1, "
                                                      "Offset y: %2\r\nWould you like to apply this offset?")
                                                       .arg(offsetX)
                                                       .arg(offsetY));
    if (res)
    {
        dhConfig->setLaserToCameraOffsetX(offsetX);
        dhConfig->setLaserToCameraOffsetY(offsetY);
    }
}

void DispenseHead::calibrateNozzelToLaserTipOffset()
{
    checkIsInit();

    anotherDispHead->dispXYZ->moveTo(XYZPosName::StandbyPos, XYZModulePos::Z_XY);
    dispXYZ->moveToPos(XYZPosName::CalibrateNozzelToLaserPos, XYZModulePos::ZS_XY_Z);

    int windowWidth = 8;

    QList<double> heights;
    QList<double> axisPoses;
    QList<double> deltaHeights;

    dispZLockCyl->set(false);
    dispZ->axis()->slowMoveTo(dhConfig->measureHeightLimitPos(), false, dhCommonConfig->measureHeightVelocityRatio());

    try
    {
        int i = 0;
        while (true)
        {
            double laserHeight = laser->readHeightNoErrorHandling();
            if (laserHeight > 200)    // Error height
            {
                throw SilicolAbort(tr("Error laser height: %1").arg(laserHeight), EX_LOCATION);
            }
            heights.append(laserHeight);
            double axisPos = dispZ->axis()->getFeedbackPos(-1);
            if (abs(axisPos - dhConfig->measureHeightLimitPos()) < 0.01)
            {
                throw SilicolAbort(tr("未检测到Laser高度不再变化!"), EX_LOCATION);
            }
            axisPoses.append(axisPos);
            if (i > 0)
            {
                deltaHeights.append(heights[i] - heights[i - 1]);
            }
            else
            {
                deltaHeights.append(0);    // 填充值，不会被使用
            }

            if (i >= windowWidth * 2)
            {
                double rightWindowSum = 0;
                double leftWindowSum = 0;
                for (int j = 0; j < windowWidth; j++)
                {
                    rightWindowSum += deltaHeights[i - j];
                    leftWindowSum += deltaHeights[i - windowWidth - j];
                }
                if (abs(leftWindowSum) / 10 >= abs(rightWindowSum))    // Laser高度不再变化，说明胶针已碰到台面
                {
                    double laserHeightWhileNozzelTouchedTable = heights[i - windowWidth];
                    nozzelToLaserTipOffsetFinally(heights, axisPoses, deltaHeights);
                    auto rsp = UIOperation::getIns()->getUIResponse(
                        "Confirm",
                        tr("Calibration nozzelToLaserTipOffset successful!\r\nlaserHeightWhileNozzelTouchedTable: "
                           "%1\r\nWould you like to apply this value?")
                            .arg(laserHeightWhileNozzelTouchedTable),
                        MsgBoxIcon::Question, OkCancelBtns);
                    if (rsp == OkBtn)
                    {
                        dhConfig->setLaserHeightWhileNozeelTouchedTable(laserHeightWhileNozzelTouchedTable);
                    }
                    break;
                }
            }
            QThread::msleep(1);
            i++;
        }
    }
    catch (SilicoolException &se)
    {
        nozzelToLaserTipOffsetFinally(heights, axisPoses, deltaHeights);
        throw se;
    }
}

void DispenseHead::calibrateShotGlueHeight(int unitRowIndex, int unitColumnIndex, int rowIndex, int columnIndex)
{
    checkIsInit();

    sensorTrayConfig->getValidUiIndex(unitRowIndex, unitColumnIndex, rowIndex, columnIndex);
    double laserHeight;
    double shotGlueHeight;
    moveToMeasureHeightImpl(unitRowIndex, unitColumnIndex, rowIndex, columnIndex, laserHeight, shotGlueHeight);
    auto res = UIOperation::getIns()->yesNoConfirm(
        tr("calibrateShotGlueHeight successful!\r\nShot glue height: %1\r\nWould you like to apply this height?")
            .arg(shotGlueHeight));
    if (res)
    {
        dhConfig->setShotGlueZHeight(shotGlueHeight);
    }
}

void DispenseHead::calibrateShotGlueDotDelay()
{
    auto xtDispenser = qobject_cast<XtDispenser *>(dispenser);
    if (xtDispenser == nullptr)
    {
        throw SilicolAbort(
            QString("Cast to XtDispenser failed! Dispenser type: %1").arg(dispenser->metaObject()->className()),
            EX_LOCATION);
    }
    moveToShotGlueDot();

    auto flyDispensingConfig = dhCommonConfig->xtFlyDispensingConfig();
    auto shotGlueDotPos = dispXYZ->getPos<XYZModulePos>(XYZPosName::ShotGlueDotPos);
    auto standbyPos = dispXYZ->getPos<XYZModulePos>(XYZPosName::StandbyPos);
    double x1 = shotGlueDotPos->xPos() - flyDispensingConfig->calibrateLineLen() / 2;
    double x2 = shotGlueDotPos->xPos() + flyDispensingConfig->calibrateLineLen() / 2;
    double y1 = shotGlueDotPos->yPos() + flyDispensingConfig->calibrateEscape();
    double y2 = shotGlueDotPos->yPos() + flyDispensingConfig->calibrateEscape() * 2;
    double shotGlueDotHeight = shotGlueDotPos->zPos() + dhCommonConfig->shotGlueZOffset();
    double safetyZHeight = standbyPos->zPos();

    xtDispenser->shotGlueDotInLineCenter(QPointF(x1, y1), QPointF(x2, y1), shotGlueDotHeight, safetyZHeight,
                                         flyDispensingConfig->shotGlueDotTime(),
                                         flyDispensingConfig->shotGlueDotDelay());
    xtDispenser->shotGlueDotInLineCenter(QPointF(x2, y2), QPointF(x1, y2), shotGlueDotHeight, safetyZHeight,
                                         flyDispensingConfig->shotGlueDotTime(),
                                         flyDispensingConfig->shotGlueDotDelay());
}

void DispenseHead::shotGlueLine(double lineLen)
{
    checkIsInit();

    double currentX = dispXYZ->xAxis()->getFeedbackPos();
    double currentY = dispXYZ->yAxis()->getFeedbackPos();
    auto standbyPos = dispXYZ->getPos<XYZModulePos>(XYZPosName::StandbyPos);
    auto shotGlueDotPos = dispXYZ->getPos<XYZModulePos>(XYZPosName::ShotGlueDotPos);
    double x1 = currentX - lineLen / 2;
    double x2 = currentX + lineLen / 2;
    double shotGlueHeight = shotGlueDotPos->zPos() + dhCommonConfig->shotGlueZOffset();
    double safetyZHeight = standbyPos->zPos();

    dispenser->dispenseLine(QPointF(x1, currentY), QPointF(x2, currentY), shotGlueHeight, safetyZHeight);
}

void DispenseHead::shotRectangle(double width, double height)
{
    checkIsInit();

    auto standbyPos = dispXYZ->getPos<XYZModulePos>(XYZPosName::StandbyPos);
    double currentX = dispXYZ->xAxis()->getFeedbackPos();
    double currentY = dispXYZ->yAxis()->getFeedbackPos();
    double shotGlueZ = dhConfig->shotGlueZHeight() + dhCommonConfig->shotGlueZOffset();

    dispenser->dispenseRect(QPointF(currentX, currentY), width, height, shotGlueZ, standbyPos->zPos());
}

double DispenseHead::getCurrentRightDispXPos()
{
    if (!isInit())
    {
        return 0;
    }
    return rightDH()->dispXYZ->xAxis()->getFeedbackPos();
}

void DispenseHead::calculateShotGlueDotDelay(double x1, double x2)
{
    auto flyDispensingConfig = dhCommonConfig->xtFlyDispensingConfig();
    flyDispensingConfig->setShotGlueDotDelay((x2 - x1) / 2 / flyDispensingConfig->calibrateMaxVel());
}

void DispenseHead::moveToPrPos(int unitRowIndex, int unitColumnIndex, int rowIndex, int columnIndex)
{
    checkIsInit();
    sensorTrayConfig->getValidUiIndex(unitRowIndex, unitColumnIndex, rowIndex, columnIndex);
    moveToPrPosImpl(unitRowIndex, unitColumnIndex, rowIndex, columnIndex);
}

void DispenseHead::saveImageForEditDispPath()
{
    checkIsInit();
    trayRawSensorLocation->camera()->saveImage(dispensePathDir(), "dispensePath.jpg");
    UIOperation::getIns()->showTip(tr("Save image to %1dispensePath.jpg").arg(dispensePathDir()));
}

QString DispenseHead::getDispensePathResultImageName()
{
    QString resultImageName = dhCommonConfig->dispensePathFileName().replace(".avdata", ".jpg");
    resultImageName = dispensePathDir() + resultImageName;
    if (QFile::exists(resultImageName))
    {
        return QString("file:///") + QFileInfo(resultImageName).absoluteFilePath();
    }
    else
    {
        return "";
    }
}

void DispenseHead::moveToMeasureHeight(int unitRowIndex, int unitColumnIndex, int rowIndex, int columnIndex)
{
    checkIsInit();
    sensorTrayConfig->getValidUiIndex(unitRowIndex, unitColumnIndex, rowIndex, columnIndex);
    double laserHeight;
    double shotGlueHeight;
    moveToMeasureHeightImpl(unitRowIndex, unitColumnIndex, rowIndex, columnIndex, laserHeight, shotGlueHeight);
    UIOperation::getIns()->showTip(tr("Move to measure height successful!\r\nLaser height: %1, Shot glue height: %2")
                                       .arg(laserHeight)
                                       .arg(shotGlueHeight));
}

void DispenseHead::moveToShotGlue(int unitRowIndex, int unitColumnIndex, int rowIndex, int columnIndex, bool doGlueCheck)
{
    checkIsInit();

    sensorTrayConfig->getValidUiIndex(unitRowIndex, unitColumnIndex, rowIndex, columnIndex);

    leftDH()->dispXYZ->moveToPos(XYZPosName::StandbyPos, XYZModulePos::Z_XY);

    QImage imageBefore;
    if (doGlueCheck)
    {
        moveToPrPosImpl(unitRowIndex, unitColumnIndex, rowIndex, columnIndex);
        imageBefore = trayRawSensorLocation->getImage();
    }

    double shotGlueHeight;
    PrOffset prOffset;
    if (dhCommonConfig->measureHeightFre()
        != DispenseHeadCommonConfig::MeasureHeightFreEnumInfo().enumToName(DispenseHeadCommonConfig::OnlyOnce))
    {
        double laserHeight;
        prOffset = moveToMeasureHeightImpl(unitRowIndex, unitColumnIndex, rowIndex, columnIndex, laserHeight,
                                           shotGlueHeight);
    }
    else
    {
        prOffset = moveToPerformPrImpl(unitRowIndex, unitColumnIndex, rowIndex, columnIndex);
        shotGlueHeight = dhConfig->shotGlueZHeight();
    }
    if (isLeftDispHead)
    {
        rightDH()->dispXYZ->moveToPos(XYZPosName::StandbyPos, XYZModulePos::Z_XY);
    }
    shotGlueHeight += dhCommonConfig->shotGlueZOffset();
    auto dispensePath = mapDispensePath(
        prOffset, rightDH()->getPrXyPos(unitRowIndex, unitColumnIndex, rowIndex, columnIndex), shotGlueHeight);
    executeDispensePath(dispensePath);

    if (doGlueCheck)
    {
        leftDH()->dispXYZ->moveToPos(XYZPosName::StandbyPos, XYZModulePos::Z_XY);
        moveToPrPosImpl(unitRowIndex, unitColumnIndex, rowIndex, columnIndex);
        QImage imageAfter = trayRawSensorLocation->getImage();
        glueCheck(imageBefore, imageAfter);
    }
}

void DispenseHead::moveToShotGlueDot()
{
    checkIsInit();

    dispXYZ->moveToWithOffset(XYZPosName::ShotGlueDotPos, XYZModulePos::ZS_XY_Z, 0, 0,
                              dhCommonConfig->shotGlueZOffset());
    shotGlueOut->set(true);
    QThread::msleep(dhCommonConfig->shotGlueDotTime());
    shotGlueOut->set(false);
    dispXYZ->moveZToPos(XYZPosName::StandbyPos);
}

void DispenseHead::readRightDH2LeftDHSafetyRange()
{
    checkIsInit();

    double rightXPos = rightDH()->dispXYZ->xAxis()->getFeedbackPos();
    double leftXPos = leftDH()->dispXYZ->xAxis()->getFeedbackPos();
    double safetyRange = rightXPos - leftXPos;
    auto res = UIOperation::getIns()->yesNoConfirm(
        tr("RightDH X pos: %1, LeftDH X pos: %2, Safety range: %3\r\nWould you like to apply this safety range?")
            .arg(rightXPos)
            .arg(leftXPos)
            .arg(safetyRange));
    if (res)
    {
        dhCommonConfig->setRightDH2LeftDHSafetyRange(safetyRange);
    }
}

void DispenseHead::performAllSensorShotGlue()
{
    performAllSensorPr();
    distributeRemainedShotGlueTask();
}

double DispenseHead::glueUsedTime() const
{
    QDateTime glueLastSetupTime = QDateTime::fromString(dhConfig->glueLastSetupTime());
    uint elapsedTime = QDateTime::currentDateTime().toTime_t() - glueLastSetupTime.toTime_t();
    return elapsedTime * 10000 / 3600 / 10000.0;
}

void DispenseHead::newGlueSetUp()
{
    dhConfig->setShotGlueTimes(0);
    dhConfig->setGlueLastSetupTime(QDateTime::currentDateTime().toString());
}

void DispenseHead::gotNewTray()
{
    hasMeasureHeightForThisTray = false;
    gotShotGlueTaskForThisTray = false;
}

void DispenseHead::distributeRemainedShotGlueTask()
{
    checkIsInit();
    if (isLeftDispHead)
    {
        throw SilicolAbort(tr("Only right dispense head is able to distributeRemainedShotGlueTask!"), EX_LOCATION);
    }

    if (disableLeftDh)
    {
        distributeAllShotGlueTaskToRightDH();
    }
    else if (disableRightDh)
    {
        distributeAllShotGlueTaskToLeftDH();
    }
    else
    {
        int startColumn = 0;
        while (startColumn < sensorTrayConfig->columns()
               && sensorTrayMap->getPrOkData(startColumn, SensorTrayMaterialData::None) == nullptr)
        {
            startColumn++;
        }
        int endColumn = sensorTrayConfig->columns() - 1;
        while (endColumn >= 0 && sensorTrayMap->getPrOkData(endColumn, SensorTrayMaterialData::None) == nullptr)
        {
            endColumn--;
        }
        if (endColumn >= startColumn)
        {
            applyLeftDHFirstDistributionStrategy(startColumn, endColumn);
        }
    }

    leftDH()->pushEvent(WorkerEvent("moveToStandbyPos"));
    if (!disableLeftDh)
    {
        rightDH()->pushEvent(WorkerEvent("moveXyzOrZToStandbyPos"));
        rightDH()->pushEvent(WorkerEvent("waitWholeTrayDoneShotGlueTask"));
    }
    else
    {
        rightDH()->pushEvent(WorkerEvent("upDispZ"));
    }

    if (executeGlueCheck)
    {
        rightDH()->pushEvent(WorkerEvent("performAllSensorGlueCheck"));
    }
    else
    {
        rightDH()->pushEvent(WorkerEvent("doDummyGlueCheck"));
        rightDH()->pushEvent(WorkerEvent("waitLeftDispZArrivedStandbyPos"));
    }

    rightDH()->pushEvent(WorkerEvent("notifyHasDoneWork"));

    rightDH()->pushEvent(WorkerEvent("tryMoveToPrLeftTopPos"));
}

void DispenseHead::performAllSensorGlueCheck()
{
    checkIsInit();
    if (isLeftDispHead)
    {
        throw SilicolAbort(tr("Only right dispense head is able to performAllSensorGlueCheck!"), EX_LOCATION);
    }

    dispImageProcessor->setImageProcessType(DispenseImageProcessor::GlueCheck);

    bool isLeft2Right = disableLeftDh;
    bool isUp2Down = false;
    QList<int> needCheckGlueColumns;
    for (int columnIndex = 0; columnIndex < sensorTrayConfig->columns(); columnIndex++)
    {
        if (sensorTrayMap->getDispOkData(columnIndex) != nullptr)
        {
            needCheckGlueColumns.append(columnIndex);
        }
    }

    dispXYZ->moveZToPos(XYZPosName::PrLeftTop, false);
    bool waitZArrivedPos = false;
    while (needCheckGlueColumns.count() > 0)
    {
        int columnIndex;
        if (isLeft2Right)
        {
            columnIndex = needCheckGlueColumns.takeFirst();
        }
        else
        {
            columnIndex = needCheckGlueColumns.takeLast();
        }

        isUp2Down = !isUp2Down;
        for (int rowIndex = 0; rowIndex < sensorTrayConfig->rows(); rowIndex++)
        {
            int realRowIndex = isUp2Down ? rowIndex : sensorTrayConfig->rows() - 1 - rowIndex;
            auto materialData = sensorTrayMap->getData<SensorTrayMaterialData>(realRowIndex, columnIndex);
            if (materialData->materialState() == SensorTrayMaterialData::DispenseOk)
            {
                QPointF targetPos = getPrXyPos(realRowIndex, columnIndex);
                dispXYZ->yAxis()->absMove(targetPos.y(), false);
                dispXYZ->xAxis()->tryToMove(targetPos.x());
                dispXYZ->yAxis()->waitArrivedPos();
                if (!waitZArrivedPos)
                {
                    dispXYZ->waitZArrivedPos(XYZPosName::PrLeftTop);
                    waitZArrivedPos = true;
                }

                QImage img = trayRawSensorLocation->getImage();
                QString indexString = index2String(realRowIndex, columnIndex);
                if (!imagesBeforeDispense.contains(indexString))
                {
                    qCCritical(logCate()) << tr("Did not find image before shoting glue! Index: (%1, %2)")
                                                 .arg(realRowIndex)
                                                 .arg(columnIndex);
                    trayRawSensorLocation->tryShowImage(img);
                    materialData->setMaterialState(SensorTrayMaterialData::DispCheckNg);
                }
                else
                {
                    dispImageProcessor->processGlueCheck(imagesBeforeDispense[indexString], img, materialData);
                }
            }
        }
    }

    imagesBeforeDispense.clear();
    while (!dispImageProcessor->wait(3000))
    {
        qCWarning(logCate()) << tr("Waiting process glue check...");
    }
}

void DispenseHead::doDummyGlueCheck()
{
    for (int columnIndex = 0; columnIndex < sensorTrayConfig->columns(); columnIndex++)
    {
        for (int rowIndex = 0; rowIndex < sensorTrayConfig->rows(); rowIndex++)
        {
            auto materialData = sensorTrayMap->getData<SensorTrayMaterialData>(rowIndex, columnIndex);
            if (materialData->materialState() == SensorTrayMaterialData::DispenseOk)
            {
                materialData->setMaterialState(SensorTrayMaterialData::DispCheckOk);
            }
        }
    }
}

void DispenseHead::notifyHasDoneWork()
{
    emit postEvent(WorkerName::DispenseWorkHolder, WorkerEvent("hasDoneWork"));
}

void DispenseHead::upDispZ()
{
    dispXYZ->moveZToPos(XYZPosName::StandbyPos);
}

void DispenseHead::moveToReadyPos()
{
    leftDH()->dispXYZ->moveTo(XYZPosName::StandbyPos, XYZModulePos::Z_XY);
    rightDH()->dispXYZ->moveToPos(XYZPosName::PrLeftTop, XYZModulePos::ZS_XY_Z);
}

void DispenseHead::waitWholeTrayDoneShotGlueTask()
{
    QElapsedTimer timer;
    timer.start();
    while (true)
    {
        if (timer.elapsed() > 20000)
        {
            qCWarning(logCate()) << tr("Waiting whole tray done work...");
            timer.restart();
        }
        if (hasAbortEvent())
        {
            return;
        }
        if (wholeTrayHasDoneShotGlueTask())
        {
            break;
        }
        QThread::msleep(30);
    }
}

void DispenseHead::waitLeftDispZArrivedStandbyPos()
{
    leftDH()->dispXYZ->waitZArrivedPos(XYZPosName::StandbyPos);
}

void DispenseHead::moveToStandbyPos()
{
    dispXYZ->moveToPos(XYZPosName::StandbyPos, XYZModulePos::Z_XY);
}

void DispenseHead::moveXyzOrZToStandbyPos()
{
    if (!wholeTrayHasDoneShotGlueTask())
    {
        rightDH()->dispXYZ->moveToPos(XYZPosName::StandbyPos, XYZModulePos::Z_XY);
    }
    else
    {
        rightDH()->dispXYZ->moveZToPos(XYZPosName::StandbyPos);
    }
}

void DispenseHead::tryMoveToPrLeftTopPos()
{
    auto prLeftTopPos = rightDH()->dispXYZ->getPos<XYZModulePos>(XYZPosName::PrLeftTop);
    rightDH()->dispXYZ->yAxis()->absMove(prLeftTopPos->yPos(), false);
    rightDH()->dispXYZ->xAxis()->tryToMove(prLeftTopPos->xPos());
    rightDH()->dispXYZ->yAxis()->waitArrivedPos(prLeftTopPos->yPos(), 0.002);
    rightDH()->dispXYZ->zAxis()->tryToMove(prLeftTopPos->zPos());
}

void DispenseHead::nozzelToLaserTipOffsetFinally(QList<double> &heights,
                                                 QList<double> &axisPos,
                                                 QList<double> &deltaHeights)
{
    dispZ->axis()->stop();
    dispZ->axis()->resetMaxVel();

    QString fileContent("AxisPos,LaserHeight,LaserDeltaHeigh\r\n");
    int length = min(heights.length(), axisPos.length());
    for (int i = 0; i < length; i++)
    {
        fileContent += QString("%1,%2,%3\r\n").arg(axisPos[i]).arg(heights[i]).arg(deltaHeights[i]);
    }
    QFile file("./NozeelToLaserCalibrationData.csv");
    if (file.open(QIODevice::ReadWrite))
    {
        file.write(fileContent.toUtf8());
        file.flush();
        file.close();
    }
    else
    {
        qDebug() << "NozeelToLaserCalibrationData";
        qDebug() << fileContent;
    }

    dispXYZ->moveZToPos(XYZPosName::StandbyPos);
    dispZLockCyl->set(true, false);
}

double DispenseHead::getAxisPosWhileLaserHeightChanged(SCAxis *axis, double targetPos, QString type)
{
    axis->slowMoveTo(targetPos, false, dhCommonConfig->laserToCameraOffsetCaliParam()->velocityRatioLimit());

    QList<double> axisPoses;
    QList<double> rawHeights;
    QList<double> filterHeights;
    QList<double> deltaHeights;

    int windowWidth = dhCommonConfig->laserToCameraOffsetCaliParam()->windowLen();

    int i = 0;
    try
    {
        while (true)
        {
            double axisPos = axis->getFeedbackPos(-1);
            if (abs(axisPos - targetPos) < 0.01)
            {
                throw SilicolAbort("未检测到Laser高度阶跃变化", EX_LOCATION);
            }
            axisPoses.append(axisPos);

            double laserHeight = laser->readHeightNoErrorHandling();
            rawHeights.append(laserHeight);
            //            if (laserHeight > 200)    // Error height
            //            {
            //                if (i > 0)
            //                {
            //                    filterHeights.append(filterHeights[i - 1]);
            //                }
            //                else
            //                {
            //                    throw SilicolAbort(tr("Error laser height: %1").arg(laserHeight), EX_LOCATION);
            //                }
            //            }
            //            else
            //            {
            //                filterHeights.append(laserHeight);
            //            }
            filterHeights.append(laserHeight);
            if (i == 0)
            {
                deltaHeights.append(0);
            }
            else
            {
                deltaHeights.append(filterHeights[i] - filterHeights[i - 1]);
            }

            if (i > windowWidth * 2)
            {
                double leftDeltaWindowSum = 0;
                double rightDeltaWindowSum = 0;
                for (int j = 0; j < windowWidth; j++)
                {
                    rightDeltaWindowSum += deltaHeights[i - j];
                    leftDeltaWindowSum += deltaHeights[i - windowWidth - j - 1];
                }
                double midDeltaValue = abs(deltaHeights[i - windowWidth]);
                double leftDeltaAve = abs(leftDeltaWindowSum) / windowWidth;
                double rightDeltaAve = abs(rightDeltaWindowSum) / windowWidth;
                if (midDeltaValue > leftDeltaAve * dhCommonConfig->laserToCameraOffsetCaliParam()->deltaRatio()
                    && midDeltaValue > rightDeltaAve * dhCommonConfig->laserToCameraOffsetCaliParam()->deltaRatio())
                {
                    bool gotIt = true;
                    if (dhCommonConfig->laserToCameraOffsetCaliParam()->cmpHeightDelta())
                    {
                        double leftHeightWindowSum = 0;
                        double rightHeightWindowSum = 0;
                        for (int j = 0; j < windowWidth; j++)
                        {
                            rightHeightWindowSum += filterHeights[i - j];
                            leftHeightWindowSum += filterHeights[i - windowWidth - j - 1];
                        }
                        if (qAbs(rightHeightWindowSum - leftHeightWindowSum) / windowWidth
                            < dhCommonConfig->laserToCameraOffsetCaliParam()->heightDelta())
                        {
                            gotIt = false;
                        }
                    }
                    if (gotIt)
                    {
                        getAxisPosWhileLaserHeightChangedFinally(axis, axisPoses, rawHeights, filterHeights,
                                                                 deltaHeights, type);
                        return axisPoses[i - windowWidth];
                    }
                }
            }

            QThread::msleep(1);
            i++;
        }
    }
    catch (SilicoolException &se)
    {
        getAxisPosWhileLaserHeightChangedFinally(axis, axisPoses, rawHeights, filterHeights, deltaHeights, type);
        throw se;
    }
}

void DispenseHead::getAxisPosWhileLaserHeightChangedFinally(SCAxis *axis,
                                                            QList<double> &axisPoses,
                                                            QList<double> &rawHeights,
                                                            QList<double> &filterHeights,
                                                            QList<double> &deltaHeights,
                                                            QString &type)
{
    axis->stop();
    axis->resetMaxVel();
    QString fileContent("AxisPos,RawLaserHeight,FilteredLaserHeight,DeltaHeight\r\n");
    int length = min(axisPoses.length(), rawHeights.length());
    for (int i = 0; i < length; i++)
    {
        fileContent += QString("%1,%2,%3,%4\r\n")
                           .arg(axisPoses[i])
                           .arg(rawHeights[i])
                           .arg(filterHeights[i])
                           .arg(deltaHeights[i]);
    }
    QFile file(QString("./LaserToCameraCalibration%1Data.csv").arg(type));
    if (file.open(QIODevice::ReadWrite))
    {
        file.write(fileContent.toUtf8());
        file.flush();
        file.close();
    }
    else
    {
        qDebug() << QString("LaserToCameraCalibration%1Data").arg(type);
        qDebug() << fileContent;
    }
}

void DispenseHead::tryToDistributeShotGlueTaskToLeftDH()
{
    if (leftDH()->hasUnhandledEvent())    //左点胶头已有点胶任务
    {
        return;
    }
    bool hasDistributedTaskToLeftDH = false;    // 如果分配任务，最多分配一列Sensor的点胶任务给左点胶头
    for (int columnIndex = 0; columnIndex < sensorTrayConfig->columns(); columnIndex++)
    {
        if (hasDistributedTaskToLeftDH)
        {
            return;
        }

        auto data = sensorTrayMap->getPrOkData(columnIndex, SensorTrayMaterialData::None);
        bool needHandleMaterial = (data != nullptr);
        if (needHandleMaterial)
        {
            SensorTrayMaterialData *lastHandledColumn = lastColumnDataHandledByLeftDH(columnIndex);
            bool isUpToDown = (lastHandledColumn == nullptr
                               || lastHandledColumn->shotGlueOrder() == SensorTrayMaterialData::DownToUp);

            bool hasCheckCollision = false;
            for (int rowIndex = 0; rowIndex < sensorTrayConfig->rows(); rowIndex++)
            {
                int realRowIndex = isUpToDown ? rowIndex : sensorTrayConfig->rows() - 1 - rowIndex;
                auto data = sensorTrayMap->getData<SensorTrayMaterialData>(realRowIndex, columnIndex);
                if (data->materialState() == SensorTrayMaterialData::DispPrOk
                    && data->willBeHandledByWhichDH() == SensorTrayMaterialData::None)
                {
                    if (!hasCheckCollision)
                    {
                        if (leftDHIsAbleToExecuteTask(data))
                        {
                            hasCheckCollision = true;
                        }
                        else
                        {
                            return;
                        }
                    }

                    data->setWillBeHandledByWhichDH(SensorTrayMaterialData::Left);
                    data->setShotGlueOrder(isUpToDown ? SensorTrayMaterialData::UpToDown
                                                      : SensorTrayMaterialData::DownToUp);

                    QVariantList shotGlueTaskParam;
                    shotGlueTaskParam << realRowIndex << columnIndex;
                    leftDH()->pushEvent(WorkerEvent("shotGlue", shotGlueTaskParam));

                    leftDH()->gotShotGlueTaskForThisTray = true;
                    hasDistributedTaskToLeftDH = true;
                }
            }
        }
    }
}

QVector<PathEndPoint> DispenseHead::mapDispensePath(SensorTrayMaterialData *sensorTrayMaterialData, double z)
{
    QVector<PathEndPoint> dispensePath;

    double theta = (sensorTrayMaterialData->dispPrOffsetT() - dhCommonConfig->initAngle()) * M_PI / 180;
    for (int i = 0; i < mechDispPath.size(); ++i)
    {
        double x = mechDispPath[i].x() * -1;
        double y = mechDispPath[i].y() * -1;
        double mappedX = x * cos(theta) - y * sin(theta) + sensorTrayMaterialData->dispPrPosX()
                         - sensorTrayMaterialData->dispPrOffsetOriginX() + dhConfig->nozeelToCameraOffsetX();
        double mappedY = y * cos(theta) + x * sin(theta) + sensorTrayMaterialData->dispPrPosY()
                         - sensorTrayMaterialData->dispPrOffsetOriginY() + dhConfig->nozeelToCameraOffsetY();

        dispensePath.push_back(PathEndPoint(mappedX, mappedY, z, LinePath));
    }
    return dispensePath;
}

QVector<PathEndPoint> DispenseHead::mapDispensePath(PrOffset prOffset, QPointF prXyPos, double z)
{
    QVector<PathEndPoint> dispensePath;

    double theta = (prOffset.Theta - dhCommonConfig->initAngle()) * M_PI / 180;
    for (int i = 0; i < mechDispPath.size(); ++i)
    {
        double x = mechDispPath[i].x() * -1;
        double y = mechDispPath[i].y() * -1;
        double mappedX
            = x * cos(theta) - y * sin(theta) + prXyPos.x() - prOffset.O_X + dhConfig->nozeelToCameraOffsetX();
        double mappedY
            = y * cos(theta) + x * sin(theta) + prXyPos.y() - prOffset.O_Y + dhConfig->nozeelToCameraOffsetY();
        dispensePath.push_back(PathEndPoint(mappedX, mappedY, z, LinePath));
    }
    return dispensePath;
}

QPointF DispenseHead::mapMeasureHeightPoint(SensorTrayMaterialData *sensorTrayMaterialData)
{
    double theta = sensorTrayMaterialData->dispPrOffsetT() * M_PI / 180;
    double x = dhCommonConfig->measureHeightXShift();
    double y = dhCommonConfig->measureHeightPosYShift();
    double mappedX = x * cos(theta) - y * sin(theta) + sensorTrayMaterialData->dispPrPosX()
                     - sensorTrayMaterialData->dispPrOffsetOriginX() + dhConfig->laserToCameraOffsetX();
    double mappedY = y * cos(theta) + x * sin(theta) + sensorTrayMaterialData->dispPrPosY()
                     - sensorTrayMaterialData->dispPrOffsetOriginY() + dhConfig->laserToCameraOffsetY();
    return QPointF(mappedX, mappedY);
}

QPointF DispenseHead::mapMeasureHeightPoint(PrOffset prOffset, QPointF prXyPos)
{
    double theta = prOffset.Theta * M_PI / 180;
    double x = dhCommonConfig->measureHeightXShift();
    double y = dhCommonConfig->measureHeightPosYShift();
    double mappedX = x * cos(theta) - y * sin(theta) + prXyPos.x() - prOffset.O_X + dhConfig->laserToCameraOffsetX();
    double mappedY = y * cos(theta) + x * sin(theta) + prXyPos.y() - prOffset.O_Y + dhConfig->laserToCameraOffsetY();
    return QPointF(mappedX, mappedY);
}

SensorTrayMaterialData *DispenseHead::lastColumnDataHandledByLeftDH(int currentColumn)
{
    currentColumn--;
    while (currentColumn >= 0)
    {
        auto data = sensorTrayMap->getPrOkData(currentColumn, SensorTrayMaterialData::Left);
        if (data != nullptr)
        {
            return data;
        }
        currentColumn--;
    }
    return nullptr;
}

SensorTrayMaterialData *DispenseHead::lastColumnDataHandledByRightDH(int currentColumn, bool isLeftToRight)
{
    if (isLeftToRight)
    {
        currentColumn--;
        while (currentColumn >= 0)
        {
            auto data = sensorTrayMap->getPrOkData(currentColumn, SensorTrayMaterialData::Right);
            if (data != nullptr)
            {
                return data;
            }
            currentColumn--;
        }
        return nullptr;
    }
    else
    {
        currentColumn++;
        while (currentColumn < sensorTrayConfig->columns())
        {
            auto data = sensorTrayMap->getPrOkData(currentColumn, SensorTrayMaterialData::Right);
            if (data != nullptr)
            {
                return data;
            }
            currentColumn++;
        }
        return nullptr;
    }
}

bool DispenseHead::needMeasureHeight(bool checkReallyMeasuredHeight)
{
    auto measureHeightFre
        = DispenseHeadCommonConfig::MeasureHeightFreEnumInfo().nameToEnum(dhCommonConfig->measureHeightFre());
    if (measureHeightFre == DispenseHeadCommonConfig::EveryMaterial)
    {
        return true;
    }
    else if (measureHeightFre == DispenseHeadCommonConfig::OnlyOnce)
    {
        return false;
    }
    else
    {
        if (checkReallyMeasuredHeight)
        {
            return !hasMeasureHeightForThisTray;
        }
        else
        {
            return !gotShotGlueTaskForThisTray;
        }
    }
}

void DispenseHead::waitIsAbleToExecuteTask(SensorTrayMaterialData *sensorTrayMaterialData)
{
    AutoResetBool tmp(&isWaitForExecuteShotGlueTask, false);
    ErrorHandler::tryToHandleGeneralError<void>([&] {
        QElapsedTimer timer;
        timer.start();
        while (true)
        {
            if (isLeftDispHead)
            {
                if (leftDHIsAbleToExecuteTask(sensorTrayMaterialData))
                {
                    break;
                }
            }
            else
            {
                if (rightDHIsAbleToExecuteTask(sensorTrayMaterialData))
                {
                    break;
                }
            }
            isWaitForExecuteShotGlueTask = true;
            if (hasAbortEvent())
            {
                return;
            }
            if (isLeftDispHead && rightDH()->isWaitForExecuteShotGlueTask)
            {
                qCWarning(logCate()) << "Dead lock detected! Try to move left dispense head!";
                leftDH()->dispXYZ->xAxis()->relMove(-20);
            }
            if (timer.elapsed() > 100000)
            {
                throw GeneralError(workerName(), "Wait another dispense head at safety pos timeout!");
            }
            QThread::msleep(30);
        }
    });
}

bool DispenseHead::leftDHIsAbleToExecuteTask(SensorTrayMaterialData *sensorTrayMaterialData)
{
    double maxXPos = getLeftDHExecuteTaskMaxPos(sensorTrayMaterialData);
    return maxXPos < leftDH()->dispXYZ->xAxis()->positiveLimit()
           && MotionManager::getIns().moveProtection()->axisIsAbleToMove(leftDH()->dispXYZ->xAxis()->name(),
                                                                         maxXPos + 5);
}

bool DispenseHead::rightDHIsAbleToExecuteTask(SensorTrayMaterialData *sensorTrayMaterialData)
{
    double minXPos = getRightDHExecuteTaskMinPos(sensorTrayMaterialData);
    return MotionManager::getIns().moveProtection()->axisIsAbleToMove(rightDH()->dispXYZ->xAxis()->name(), minXPos - 5);
}

double DispenseHead::getLeftDHExecuteTaskMaxPos(SensorTrayMaterialData *sensorTrayMaterialData)
{
    auto dispensePath = leftDH()->mapDispensePath(sensorTrayMaterialData, 0);
    double maxXPos = INT_MIN;
    for (int i = 0; i < dispensePath.length(); i++)
    {
        if (dispensePath[i].x > maxXPos)
        {
            maxXPos = dispensePath[i].x;
        }
    }
    if (leftDH()->needMeasureHeight(false))
    {
        QPointF measureHeightPos = leftDH()->mapMeasureHeightPoint(sensorTrayMaterialData);
        maxXPos = max(maxXPos, measureHeightPos.x());
    }
    return maxXPos;
}

double DispenseHead::getRightDHExecuteTaskMinPos(SensorTrayMaterialData *sensorTrayMaterialData)
{
    auto dispensePath = mapDispensePath(sensorTrayMaterialData, 0);
    double minXPos = INT_MAX;
    for (int i = 0; i < dispensePath.length(); i++)
    {
        if (dispensePath[i].x < minXPos)
        {
            minXPos = dispensePath[i].x;
        }
    }
    if (needMeasureHeight(false))
    {
        QPointF measureHeightPos = mapMeasureHeightPoint(sensorTrayMaterialData);
        minXPos = min(minXPos, measureHeightPos.x());
    }
    return minXPos;
}

void DispenseHead::distributeAllShotGlueTaskToRightDH()
{
    for (int columnIndex = sensorTrayConfig->columns() - 1; columnIndex >= 0; columnIndex--)
    {
        bool isUpToDown;
        SensorTrayMaterialData *lastHandledColumn = lastColumnDataHandledByRightDH(columnIndex, false);
        if (lastHandledColumn == nullptr)
        {
            isUpToDown = (sensorTrayConfig->columns() - 1) % 2 == 1;
        }
        else
        {
            isUpToDown = (lastHandledColumn->shotGlueOrder() == SensorTrayMaterialData::DownToUp);
        }
        for (int rowIndex = 0; rowIndex < sensorTrayConfig->rows(); rowIndex++)
        {
            int realRowIndex = isUpToDown ? rowIndex : sensorTrayConfig->rows() - rowIndex - 1;
            auto data = sensorTrayMap->getData<SensorTrayMaterialData>(realRowIndex, columnIndex);
            if (data->materialState() == SensorTrayMaterialData::DispPrOk
                && data->willBeHandledByWhichDH() == SensorTrayMaterialData::None)
            {
                data->setWillBeHandledByWhichDH(SensorTrayMaterialData::Right);
                data->setShotGlueOrder(isUpToDown ? SensorTrayMaterialData::UpToDown
                                                  : SensorTrayMaterialData::DownToUp);

                QVariantList shotGlueTaskParam;
                shotGlueTaskParam << realRowIndex << columnIndex;
                pushEvent(WorkerEvent("shotGlue", shotGlueTaskParam));
            }
        }
    }
}

void DispenseHead::distributeAllShotGlueTaskToLeftDH()
{
    for (int columnIndex = 0; columnIndex < sensorTrayConfig->columns(); columnIndex++)
    {
        if (sensorTrayMap->getPrOkData(columnIndex, SensorTrayMaterialData::Left) != nullptr)
        {
            break;
        }
        SensorTrayMaterialData *lastHandledColumn = lastColumnDataHandledByLeftDH(columnIndex);
        bool isUpToDown
            = (lastHandledColumn == nullptr || lastHandledColumn->shotGlueOrder() == SensorTrayMaterialData::DownToUp);
        for (int rowIndex = 0; rowIndex < sensorTrayConfig->rows(); rowIndex++)
        {
            int realRowIndex = isUpToDown ? rowIndex : sensorTrayConfig->rows() - 1 - rowIndex;
            auto data = sensorTrayMap->getData<SensorTrayMaterialData>(realRowIndex, columnIndex);
            if (data->materialState() == SensorTrayMaterialData::DispPrOk
                && data->willBeHandledByWhichDH() == SensorTrayMaterialData::None)
            {
                data->setWillBeHandledByWhichDH(SensorTrayMaterialData::Left);
                data->setShotGlueOrder(isUpToDown ? SensorTrayMaterialData::UpToDown
                                                  : SensorTrayMaterialData::DownToUp);

                QVariantList shotGlueTaskParam;
                shotGlueTaskParam << realRowIndex << columnIndex;
                leftDH()->pushEvent(WorkerEvent("shotGlue", shotGlueTaskParam));

                leftDH()->gotShotGlueTaskForThisTray = true;
            }
        }
    }
}

void DispenseHead::applyLeftDHFirstDistributionStrategy(int startColumn, int endColumn)
{
    int midColumn = (startColumn + endColumn + 1) / 2;
    auto leftData = sensorTrayMap->getPrOkData(startColumn, SensorTrayMaterialData::None);
    if (leftData == nullptr)
    {
        throw SilicolAbort("Logic error! applyLeftDHFirstDistributionStrategy", EX_LOCATION);
    }

    // 左点胶头当前执行任务最大位置，与右点胶头第一个任务最小位置不得干涉
    while (midColumn <= endColumn)
    {
        auto rightData = sensorTrayMap->getPrOkData(midColumn, SensorTrayMaterialData::None);
        if (rightData != nullptr)
        {
            double leftDHMaxPos = getLeftDHExecuteTaskMaxPos(leftData);
            double rightDHMinPos = getRightDHExecuteTaskMinPos(rightData);
            if (rightDHMinPos - leftDHMaxPos > dhCommonConfig->rightDH2LeftDHSafetyRange() + 10)
            {
                break;
            }
        }
        midColumn++;
    }

    // 左点胶头最后一个任务最大位置不得超过软限位
    while (midColumn - 1 >= startColumn)
    {
        auto data = sensorTrayMap->getPrOkData(midColumn - 1, SensorTrayMaterialData::None);
        if (data != nullptr)
        {
            double leftDHMaxPos = getLeftDHExecuteTaskMaxPos(data);
            if (leftDHMaxPos + 5 < leftDH()->dispXYZ->xAxis()->positiveLimit())
            {
                break;
            }
        }
        midColumn--;
    }

    for (int columnIndex = startColumn; columnIndex < midColumn; columnIndex++)
    {
        SensorTrayMaterialData *lastHandledColumn = lastColumnDataHandledByLeftDH(columnIndex);
        bool isUpToDown
            = (lastHandledColumn == nullptr || lastHandledColumn->shotGlueOrder() == SensorTrayMaterialData::DownToUp);
        for (int rowIndex = 0; rowIndex < sensorTrayConfig->rows(); rowIndex++)
        {
            int realRowIndex = isUpToDown ? rowIndex : sensorTrayConfig->rows() - 1 - rowIndex;
            auto data = sensorTrayMap->getData<SensorTrayMaterialData>(realRowIndex, columnIndex);
            if (data->materialState() == SensorTrayMaterialData::DispPrOk
                && data->willBeHandledByWhichDH() == SensorTrayMaterialData::None)
            {
                data->setWillBeHandledByWhichDH(SensorTrayMaterialData::Left);
                data->setShotGlueOrder(isUpToDown ? SensorTrayMaterialData::UpToDown
                                                  : SensorTrayMaterialData::DownToUp);

                QVariantList shotGlueTaskParam;
                shotGlueTaskParam << realRowIndex << columnIndex;
                leftDH()->pushEvent(WorkerEvent("shotGlue", shotGlueTaskParam));

                leftDH()->gotShotGlueTaskForThisTray = true;
            }
        }
    }

    for (int columnIndex = midColumn; columnIndex <= endColumn; columnIndex++)
    {
        bool isUpToDown;
        SensorTrayMaterialData *lastHandledColumn = lastColumnDataHandledByRightDH(columnIndex, true);
        if (lastHandledColumn == nullptr)
        {
            isUpToDown = (sensorTrayConfig->columns() - 1) % 2 == 1;
        }
        else
        {
            isUpToDown = (lastHandledColumn->shotGlueOrder() == SensorTrayMaterialData::DownToUp);
        }
        for (int rowIndex = 0; rowIndex < sensorTrayConfig->rows(); rowIndex++)
        {
            int realRowIndex = isUpToDown ? rowIndex : sensorTrayConfig->rows() - 1 - rowIndex;
            auto data = sensorTrayMap->getData<SensorTrayMaterialData>(realRowIndex, columnIndex);
            if (data->materialState() == SensorTrayMaterialData::DispPrOk
                && data->willBeHandledByWhichDH() == SensorTrayMaterialData::None)
            {
                data->setWillBeHandledByWhichDH(SensorTrayMaterialData::Right);
                data->setShotGlueOrder(isUpToDown ? SensorTrayMaterialData::UpToDown
                                                  : SensorTrayMaterialData::DownToUp);

                QVariantList shotGlueTaskParam;
                shotGlueTaskParam << realRowIndex << columnIndex;
                pushEvent(WorkerEvent("shotGlue", shotGlueTaskParam));
            }
        }
    }
}

void DispenseHead::moveToPrPosImpl(int unitRowIndex, int unitColumnIndex, int rowIndex, int columnIndex)
{
    QPointF xyPos = rightDH()->getPrXyPos(unitRowIndex, unitColumnIndex, rowIndex, columnIndex);
    rightDH()->dispXYZ->moveZToPos(XYZPosName::PrLeftTop);
    rightDH()->dispXYZ->moveXY(xyPos.x(), xyPos.y());
}

PrOffset DispenseHead::moveToPerformPrImpl(int unitRowIndex, int unitColumnIndex, int rowIndex, int columnIndex)
{
    moveToPrPosImpl(unitRowIndex, unitColumnIndex, rowIndex, columnIndex);
    PrOffset prOffset;
    if (rightDH()->trayRawSensorLocation->performPR(prOffset))
    {
        return prOffset;
    }
    else
    {
        throw SilicolAbort(tr("trayRawSensorLocation Pr failed!"));
    }
}

PrOffset DispenseHead::moveToMeasureHeightImpl(
    int unitRowIndex, int unitColumnIndex, int rowIndex, int columnIndex, double &laserHeight, double &shotGlueHeight)
{
    if (isLeftDispHead)
    {
        dispXYZ->moveTo(XYZPosName::StandbyPos, XYZModulePos::Z_XY);
    }
    PrOffset prOffset = moveToPerformPrImpl(unitRowIndex, unitColumnIndex, rowIndex, columnIndex);
    if (isLeftDispHead)
    {
        rightDH()->dispXYZ->moveTo(XYZPosName::StandbyPos, XYZModulePos::Z_XY);
    }
    QPointF measureHeightPos
        = mapMeasureHeightPoint(prOffset, getPrXyPos(unitRowIndex, unitColumnIndex, rowIndex, columnIndex));
    dispXYZ->moveZToPos(XYZPosName::StandbyPos);
    dispXYZ->moveXY(measureHeightPos.x(), measureHeightPos.y());
    dispZ->moveTo(SAxisPosName::MeasureHeightPos);
    QThread::msleep(100);
    laserHeight = laser->readValidHeight();
    double currentZ = dispZ->axis()->getFeedbackPos(-1);
    shotGlueHeight = currentZ - (laserHeight - dhConfig->laserHeightWhileNozeelTouchedTable());
    return prOffset;
}

void DispenseHead::executeDispensePath(QVector<PathEndPoint> &dispensePath)
{
    if (dispensePath.length() < 3)
    {
        throw SilicolAbort(tr("dispensePath size is too small! Size: %1").arg(dispensePath.length()), EX_LOCATION);
    }
    double readyForDispenseHeight = dispensePath[0].z + dhCommonConfig->readyForDispenseZOffset();
    dispXYZ->zAxis()->absMove(readyForDispenseHeight, false);
    dispXYZ->moveXY(dispensePath[0].x, dispensePath[0].y);
    dispXYZ->zAxis()->waitArrivedPos(readyForDispenseHeight, 0.002);

    try
    {
        dispenser->dispensePath(dispensePath);
        dispZ->axis()->absMove(readyForDispenseHeight);
        dhConfig->setShotGlueTimes(dhConfig->shotGlueTimes() + 1);
    }
    catch (SilicoolException &se)
    {
        dispZ->axis()->absMove(readyForDispenseHeight);
        throw se;
    }
}

QPointF DispenseHead::getPrXyPos(int rowIndex, int columnIndex)
{
    int unitRowIndex = rowIndex / sensorTrayConfig->rowsInsideUnit();
    int rowIndexInsideUnit = rowIndex % sensorTrayConfig->rowsInsideUnit();
    int unitColumnIndex = columnIndex / sensorTrayConfig->columnsInsideUnit();
    int columnIndexInsideUnit = columnIndex % sensorTrayConfig->columnsInsideUnit();
    return getPrXyPos(unitRowIndex, unitColumnIndex, rowIndexInsideUnit, columnIndexInsideUnit);
}

QPointF DispenseHead::getPrXyPos(int unitRowIndex, int unitColumnIndex, int rowIndex, int columnIndex)
{
    auto prLTPos = rightDH()->dispXYZ->getPos<XYZModulePos>(XYZPosName::PrLeftTop);

    double deltaX
        = unitColumnIndex * sensorTrayConfig->unitColumnDelta() + columnIndex * sensorTrayConfig->columnDelta();
    double deltaY = unitRowIndex * sensorTrayConfig->unitRowDelta() + rowIndex * sensorTrayConfig->rowDelta();
    return QPointF(prLTPos->xPos() + deltaX, prLTPos->yPos() - deltaY);
}

bool DispenseHead::wholeTrayHasDoneShotGlueTask()
{
    for (int columnIndex = 0; columnIndex < sensorTrayConfig->columns(); columnIndex++)
    {
        for (int rowIndex = 0; rowIndex < sensorTrayConfig->rows(); rowIndex++)
        {
            auto materialData = sensorTrayMap->getData<SensorTrayMaterialData>(rowIndex, columnIndex);
            if (materialData->materialState() == SensorTrayMaterialData::DispPrNg
                || materialData->materialState() == SensorTrayMaterialData::DispenseNg
                || materialData->materialState() == SensorTrayMaterialData::DispenseOk)
            {
                continue;
            }
            else
            {
                return false;
            }
        }
    }
    return true;
}

QString DispenseHead::index2String(int rowIndex, int columnIndex)
{
    return QString("%1_%2").arg(rowIndex).arg(columnIndex);
}

QPoint DispenseHead::string2Index(QString s)
{
    auto indexes = s.split("_");
    if (indexes.count() != 2)
    {
        throw SilicolAbort(tr("Unknown index string! %1").arg(s), EX_LOCATION);
    }
    return QPoint(indexes[0].toInt(), indexes[1].toInt());
}

bool DispenseHead::glueCheck(QImage &imageBefore, QImage &imageAfter)
{
    auto glueCheckParam = dhCommonConfig->glueCheckParam();
    QImage resultImage;
    double outMaxGlueWidth = 0;
    double outMinGlueWidht = 0;
    double outAveGlueWidth = 0;
    bool res
        = VisionManager::getIns().vision()->glueCheck(imageBefore, imageAfter, glueCheckParam->resoultion(),
                                                      resultImage, outMinGlueWidht, outMaxGlueWidth, outAveGlueWidth);
    if (res)
    {
        trayRawSensorLocation->tryShowImage(resultImage);
        if (outMaxGlueWidth <= glueCheckParam->maxGlueWidth() && outMinGlueWidht >= glueCheckParam->minGlueWidth()
            && outAveGlueWidth >= glueCheckParam->aveGlueWidthLowerLimit()
            && outAveGlueWidth <= glueCheckParam->aveGlueWidthUpperLimit())
        {
            return true;
        }
        else
        {
            qCInfo(logCate()) << tr("Judge glue width failed! Glue width limit: [%1, %2], real glue width range: [%3, "
                                    "%4].\r\n"
                                    "Average glue width limit: [%5, %6], average glue width: %7")
                                     .arg(glueCheckParam->minGlueWidth())
                                     .arg(glueCheckParam->maxGlueWidth())
                                     .arg(outMinGlueWidht)
                                     .arg(outMaxGlueWidth)
                                     .arg(glueCheckParam->aveGlueWidthLowerLimit())
                                     .arg(glueCheckParam->aveGlueWidthUpperLimit())
                                     .arg(outAveGlueWidth);
            return false;
        }
    }
    else
    {
        qCWarning(logCate()) << tr("Execute glue check failed!");
        trayRawSensorLocation->tryShowImage(imageAfter);
        return false;
    }
}

DispenseHeadConfig *DispenseHead::getDhConfig() const
{
    return dhConfig;
}

void DispenseHead::readDispensePath()
{
    if (trayRawSensorCalibration == nullptr)
    {
        throw SilicolAbort("trayRawSensorCalibration is null!", EX_LOCATION);
    }
    QString fileName = dispensePathDir() + dhCommonConfig->dispensePathFileName();
    if (!QFile::exists(fileName))
    {
        throw SilicolAbort(tr("Dispense path file not exist! FileName: %1").arg(fileName));
    }
    try
    {
        QVector<QPointF> pixelPath = VisionManager::getIns().vision()->readDispensePath(fileName);
        mechDispPath.clear();
        foreach (auto point, pixelPath)
        {
            QPointF mechPoint;
            if (!trayRawSensorCalibration->getDeltaDistanceFromCenter(point, mechPoint))
            {
                throw SilicolAbort("trayRawSensorCalibration is not inited!", EX_LOCATION);
            }
            mechDispPath.push_back(QPointF(mechPoint.x(), mechPoint.y()));
        }
        anotherDispHead->mechDispPath = this->mechDispPath;
    }
    catch (SilicoolException &se)
    {
        anotherDispHead->mechDispPath = this->mechDispPath;
        throw se;
    }
}
